/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.templates.commands.ManualDrive;

/**
 *
 * @author Jarrod Risley
 */
public class PIDSecondaryArmSubsystem extends PIDSubsystem {

    /*
     * Global Declarations
     */
    Victor secondaryArmVictor;
    AnalogChannel secondaryPot;
    
    private static final double Kp = 0.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;

    private boolean victorEnabled = false;
    private double lastMoveTime = 0.0;
    private double timeToTraverseACount = 0.015;
    private double minImpulseDuration = 0.5;
    private double controlImpulseDuration = 3.0;
    String armName;
    
    /**
     * These values are for the second arm - a-k-a the small arm attached to
     * the bigger arm.
     */
    
    public static class SecondaryArmPosition {
        
        /*
         * Local Variables
         * 
         * ...These need to be calibrated.
         */
        
        private double value;
        private static final double LEVEL = 0;
        // W/R/T being level with the main arm...
        private static final double FORTY_FIVE_ABOVE = 0;
        private static final double FORTY_FIVE_BELOW= 0;
        private static final double NINETY_ABOVE = 0;
        private static final double NINETY_BELOW = 0;
        
        public static final SecondaryArmPosition LEVEL_WITH_ARM =
                new SecondaryArmPosition(LEVEL);
        
        public static final SecondaryArmPosition FORTY_FIVE_ABOVE_LEVEL =
                new SecondaryArmPosition(FORTY_FIVE_ABOVE);
        
        public static final SecondaryArmPosition FORTY_FIVE_BELOW_LEVEL =
                new SecondaryArmPosition(FORTY_FIVE_BELOW);
        
        public static final SecondaryArmPosition NINETY_ABOVE_LEVEL = 
                new SecondaryArmPosition(NINETY_ABOVE);
        
        public static final SecondaryArmPosition NINTEY_BELOW_LEVEL = 
                new SecondaryArmPosition(NINETY_BELOW);
        
        /**
         * Constructs a SecondaryArmPosisition - a specified potentiometer value.
         * 
         * @param position The potentiometer value that corresponds to a 
         * desired height for the arm.
         */
        
        private SecondaryArmPosition(double position) {
            this.value = position;
        } // end SecondaryArmPosition
        
    } // end SecondaryArmPosition
    
    /**
     * Sets the default command for this subsystem.
     */
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        
        setDefaultCommand(new ManualDrive());
        
    } // end initDefaultCommand
    
    /**
     * Grabs the potentiometer value from the secondary arm.
     * 
     * @return The potentiometer value as a double.
     */
    
    public double getSecondaryArmPotValue() {
        return returnPIDInput();
    } // end getSecondaryArmPosition
    
    /**
     * Does the slog-work for the function above.
     * @return 
     */
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        
        double position = secondaryPot.getAverageValue();
        
        return position;
        
    } // end returnPIDInput
    
    /**
     * Checks to see if the victor has been enabled.
     * 
     * @return True if enabled.
     */
    
    public boolean isMotorEnabled() {
        return victorEnabled;
    } // end isMotorEnabled
    
    /**
     * Sets the position for the secondary arm.
     * 
     * @param position A SecondaryArmPosition to move the arm to a preset
     * location.
     */
    
    public void setSecondaryArmPosition(SecondaryArmPosition position) {
        
        enable();
        victorEnabled = true;
        controlImpulseDuration = minImpulseDuration + timeToTraverseACount
                * Math.abs(position.value - getSecondaryArmPotValue());
        lastMoveTime = Timer.getFPGATimestamp();
        setSetpoint(position.value);
        
    } // end setSecondaryArmPosition
    
    /**
     * Still not sure how this is implemented...
     * 
     * @param output Something that drives a motor (-_-)
     */
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        
        secondaryArmVictor.set(output);
        
        if ((victorEnabled)
                && (Timer.getFPGATimestamp() > (lastMoveTime + controlImpulseDuration))) {
            // NOTE: This order is important because disable()ing the PID controller stops immediately, it does
            //       not allow the function to finish
            //System.out.println("Disabling Lifter Motor " + lifterName + " " + m_Lifter + " @ " + lastEnableCommand);
            victorEnabled = false;
            disable();
            
        } // end if
        
    } // end usePIDOutput
    
    /**
     * Allows the user to control the secondary arm via joystick control.
     * 
     * @param input The joystick value.
     */
    
    public void manualDrive(double input) {
        secondaryArmVictor.set(input);
    } // end manualDrive
    
    /*
     * Constructor
     */
    
    public PIDSecondaryArmSubsystem(String name, int secondVic, int secPot) {
        super(name, Kp, Ki, Kd);

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
        
        armName = name;
        
        secondaryArmVictor = new Victor(secondVic);
        secondaryPot = new AnalogChannel(secPot);
        
        enable();
        
        System.out.println(armName + " has been constructed.");
        
    } // end PIDSecondaryArmSubsystem Constructor
    
} // end PIDSecondaryArmSubsystem 
